#coding: utf-8
from peanut_app import PeanutAPP
from peanut_ros import PeanutROS
import uiautomator2 as u2
import time
import re
import logging
import sys
import os
import atexit
import traceback
from roslibpy import Ros
#Python ROS Bridge library，简称 roslibpy，提供了用 Python 或 IronPython 与开源机器人平台 ROS 进行通信的一个途径
import signal
from traceback import *

ANDROID_IP_ADDRESS = '192.168.100.22'
ROS_MASTER_IP_ADDRESS = '192.168.100.21'
CENTER_IP_ADDRESS = '192.168.100.10'


class Peanut:
    def __init__(self, app: PeanutAPP, ros: PeanutROS):
        # logger
        LOG_FORMAT = "%(asctime)s[PEANUT]%(message)s"
        DATE_FORMAT = "[%Y/%m/%d %H:%M:%S]"

        self.logger = logging.getLogger('peanut')
        self.logger.setLevel(logging.INFO)
        formatter = logging.Formatter(fmt=LOG_FORMAT, datefmt=DATE_FORMAT)
        handler = logging.StreamHandler(sys.stdout)
        handler.setFormatter(formatter)
        self.logger.addHandler(handler)

        self.app = app
        self.ros = ros
        self.center = Ros(CENTER_IP_ADDRESS, 9090)
        self.center.run()
        if not self.center.is_connected:
            self.logger.error("无法连接到中心系统")
            sys.exit(-1)
        else:
            self.logger.info("连接到中心系统")

        # self.stop_charge()
        self.in_charge_process = False

        # 获取目标点列表、目标点名称列表
        self.targets = []
        self.target_dict = {}
        self.update_targets()

        self.in_cabin = False

        # target related parameters target_name:送餐目标名
        self.current_target = {
            'target_name': "",
            'phone_number': "17700000000"
        }

        # 标记花生是否在移动
        self.on_moving = False
        # 标记当前是否有订单
        self.order_ready = False 

        # 简单的手机号判断
        self.phone_re = re.compile(r"^[1][3-9][0-9]{9}$")
        
        # 记录送餐之前的托盘重量
        self.init_weight = self.ros.cur_weighing

        # watchers
        # self.app.device.watcher('RETURN').when('返回服务台').call(
        #     lambda : self.into_cabin() 
        # )
        # self.app.device.watcher('OPEN_HATCH').when('开门中，请稍等…').call(
        #     lambda : self.app.set_hatch_status(True)
        # )
        # self.app.device.watcher('CLOSE_HATCH').when('关门中，请稍等…').call(
        #     lambda : self.app.set_hatch_status(False)
        # )
        # self.app.device.watcher('FETCH').when('开门取物').call(self.app.open_hatch_on_delivered)

        # watchContext
        # self.ctx = self.app.device.watch_context()

        # for sig in [signal.SIGTERM, signal.SIGINT, signal.SIGHUP, signal.SIGQUIT]:
        #     signal.signal(sig, self.signal_handler)
        #     signal.siginterrupt(sig, False)

        self.logger.info('Peanut 初始化完成')

    def update_targets(self):
        """更新目标点数组
        """
        self.targets = self.ros.get_target_points()
        for t in self.targets:
            self.target_dict[t['name']] = t['pose']

    def delivery(self, target_name: str, phone_number: str=None, pose: dict = None):
        """通过 APP 调用送物

        Args:
            target_name (str): 目标名
            phone_number (str): 手机号
            pose (dict, optional): 如果目标是"送餐点"，传入这个参数以动态改变送餐位置
        """
        if target_name not in self.target_dict.keys():
            self.logger.error('非法目标点')
            return
        if phone_number:
            if not self.phone_re.match(phone_number):
                # 手机号非法
                self.logger.error('非法手机号')
                return
        else:
            # 合法目标点和合法手机号，执行送物流程
            self.current_target['target_name'] = target_name
            self.current_target['phone_number'] = phone_number
            self.stop_charge()
            # 动态自定义目标点
            if target_name == '送餐点' and pose:
                self.modify_dynamic_target(pose)
                self.update_targets()
            # 通过 app 模拟点击执行送物流程
            self.app.delivery(target_name=target_name, phone_number=phone_number, wait=False)

    def dispatch(self, target_name: str, pose: dict = None):
        """通过 APP 执行派遣

        Args:
            target_name (str): 目标名
            pose (dict, optional): 如果 target_name 是"送餐点"，传入这个参数以动态改变送餐位置
        """
        if target_name not in self.target_dict.keys():
            self.logger.error('非法目标点')
            return
        else:
            # 合法目标点，执行派遣流程
            self.stop_charge()
            # 动态自定义目标点
            if target_name == '送餐点' and pose:
                self.modify_dynamic_target(pose)
                self.update_targets()
            # 通过 app 模拟点击执行派遣流程
            self.app.dispatch(target_name=target_name)

    def modify_dynamic_target(self, pose):
        # @TODO: NOT USED in current version
        """临时修改送餐点

        Args:
            pose (geometry_msgs/Pose): 送餐位置的坐标，格式是 geometry_msgs/Pose
        """
        target = [target for target in self.targets if target['name'] == '送餐点'][0]
        self.ros.delete_target_point(target)
        target['pose'] = pose
        self.ros.insert_target_point(target)
        self.logger.info('Modify dynamic target pose to: {}'.format(pose))
    
    def charge(self):
        """调用 app 命令机器人返回充电
        """
        if self.in_charge_process:
            return
        self.in_charge_process = True
        if self.in_cabin:
            out = self.out_cabin()
        else:
            out = True
        if out:
            self.app.charge()
        else:
            self.logger.error('机器人在舱体内, 无法返回充电.')
        self.in_charge_process = False

    def stop_charge(self):
        """调用 APP 和 ROS 停止机器人充电
        """
        charge_stopped = self.app.stop_charge()
        if not charge_stopped:
            self.ros.stop_charge()

    def start_watch(self, use_watcher=True):
        if use_watcher:
            self.app.device.watcher.start()
        else:
            self.ctx = self.app.device.watch_context()
            self.ctx.when('开门中，请稍等…').call(
                lambda : self.app.set_hatch_status(True)
            )
            self.ctx.when('关门中，请稍等…').call(
                lambda : self.app.set_hatch_status(False)
            )
            self.ctx.when('开门取物').call(self.app.open_hatch_on_delivered)

    def stop_watch(self, use_watcher=True):
        if use_watcher:
            self.app.device.watcher.stop()
        else:
            self.ctx.close()

    def into_cabin(self):
        """控制机器人进入舱体

        Returns:
            bool: 是否顺利进入舱体
        """
        # 导航到舱体前
        if self.in_cabin:
            return True
        at_cabin1 = self.stand_by()
        if not at_cabin1:
            self.logger.error('无法到达舱体前')
            self.in_cabin = False
            return False

        # 进入舱体
        self.ros.move_to(self.target_dict['cabin2'])
        for _ in range(60):
            if self.ros.goal_reached:
                self.logger.info('进入舱体内部')
                break
            else:
                time.sleep(1)

        if self.ros.goal_reached:
            self.ros.goal_reached = False
            time.sleep(1)
        else:
            self.logger.error('无法进入舱体内部')
            self.in_cabin = False
            return False

        # 前进 15 厘米
        self.ros.move_by(dis=0.2, linear_speed=0.05)
        self.logger.info('到达取咖啡位置')
        time.sleep(1)
        self.in_cabin = True

        # 打开 APP 主界面
        self.app.launch_delivery_main_menu_page()
        return True

    def out_cabin(self, stand_by=False):
        """控制机器人离开舱体到舱体前的位置待机

        Returns:
            bool: True 表示顺利到达待机位置，否则为 False
        """
        self.ros.move_by(dis=-0.2, linear_speed=0.05)
        self.ros.move_by(angle=-90, angular_speed=0.5)
        self.ros.move_by(dis=-0.2, linear_speed=0.05)
        self.ros.move_by(angle=180, angular_speed=0.5)
        self.in_cabin = False
        if stand_by:
            return self.stand_by()
        else:
            self.logger.info('花生离开舱体')
            
            return True

    def stand_by(self, retry=10):
        self.stop_charge()
        if self.in_cabin:
            self.out_cabin()
        # self.app.launch_delivery_main_menu_page()
        for _ in range(retry):
            self.ros.move_to(self.target_dict['cabin1'])
            for _ in range(30):
                if self.ros.goal_reached:
                    self.logger.info('花生到达待机位置')

                    return True
                else:
                    time.sleep(3)
            self.logger.error('花生无法到达待机位置, 重试中...')
        self.logger.error('花生无法到达待机位置, 执行返回充电...')
        self.charge()
        return False

    def move(self, dis, speed):
        """move forward via ros

        Args:
            dis (float): 前进距离, 单位米.负数表示倒退.
            speed (float): 线速度, 单位 m/s.
        """
        self.ros.move_by(dis=dis, linear_speed=speed)

    def rotate(self, angle, speed):
        """rotate via ros

        Args:
            angle (float): 旋转角度, 单位为度数. 正数为逆时针旋转, 负数为顺时针旋转.
            speed (float): 角速度, 单位 rad/s.
        """

        self.ros.move_by(angle=angle, angular_speed=speed)

    def move_to(self, target_name):
        """使用 ROS 移动到指定的 target

        Args:
            target_name (string): target name, 也就是 self.target_dict 的 key
        """
        if target_name in self.target_dict.keys():
            self.ros.move_to(self.target_dict[target_name])
        else:
            self.logger.error('非法目标点')

    def stop_move(self):
        """stop robot to move via ros
        """
        self.ros.stop_move()

    def open_hatch(self):
        """open hatch via app
        """
        self.app.open_hatch()

    def close_hatch(self):
        """close hatch via app
        """
        self.app.close_hatch()

    def signal_handler(self, sig, frame):
        print('Ctrl-C triggered.')
        self.charge()
        sys.exit(0)

    def wait_for_order(self, wait_time=None, interval=10):
        """等待订单

        Args:
            wait_time (int, optional): 等待时间, 单位为分钟. 传入 None 则持续等待. 默认为 None.
            interval (int, optional): 查询订单间隔时间, 单位为秒. 默认 10 秒.
        """
        self.logger.info('等待新订单...')
        wait_count = wait_time * 60 if wait_time is not None else None
        ready = None
        
        while not wait_count or wait_count > 0:
            while not self.ros.client.is_connected:
                try:
                    self.ros.client.terminate()
                    time.sleep(5)
                    self.ros.client.run()
                except:
                    self.logger.error("无法连接到 ROS, 重试中...")

            try:
                ready = self.center.get_param('/order/ready')
                if ready in ['no', None]:
                    self.app.swipe() # 保活屏幕
                else:
                    break
            except:
                traceback.print_exc()
                self.logger.error('获取 ROS 参数 "/order/ready" 时出错')
                self.center.terminate()
                time.sleep(5)
                self.center.run()
                if self.center.is_connected:
                    self.logger.info('重新连接到中心系统')
                else:
                    self.logger.error('无法重新连接到中心系统')
                self.app.swipe() # 保活屏幕
                
            time.sleep(interval)
            if wait_count is not None:
                wait_count -= interval
                # self.logger.info('等待中 %ds...' % wait_count)
        
        if ready == 'yes':
            self.logger.info('接到新订单')
            self.center.set_param('/order/ready', 'no')     # 重置订单准备标记
            self.logger.info('[SEND SIGNAL] /order/ready, \'no\' ')
            return True
        else:
            self.logger.info('没有新订单, 返回充电')
            return False

    def get_coffee(self, target_name=None, phone_number=None, use_app=True):
        """获取咖啡

        Args:
            target_name (str, optional): 目标名
            phone_number (str, optional): 手机号
            use_app (bool, optional): 是否使用 APP

        Returns:
            bool: 是否成功取到咖啡， 是为 True, 否则为 False
        """
        self.stop_charge()
        if not self.in_cabin:
            self.into_cabin()
        if self.in_cabin:
            if use_app:
                self.app.delivery_wait()
            else:
                self.app.open_hatch()
            
            # 记录初始重量
            self.init_weight = self.ros.cur_weighing
            self.center.set_param('/peanut/ready', 'yes')   # 告知机械臂花生已就位
            self.logger.info('[SEND SIGNAL] /peanut/ready, \'yes\' ')
        
            # 若机械臂未放置好咖啡，点击屏幕保持上盖开启
            # 最多等待 300 秒，否则直接离开
            timeout = 300
            while self.center.get_param('/peanut/coffee_place') in ['no', None] and timeout > 0:
                self.app.device.click(0, 0)
                timeout -= 3
                time.sleep(3)
            # self.logger.info('[GET SIGNAL] /peanut/coffee_place, \'yes\' ')

            if target_name is None: # 等待中心系统告知桌号
                target_name = str(self.center.get_param('/peanut/table_id'))
            assert target_name is not None, "Target name is None."
            # 多目标用 '_' 分隔, 先到的订单放后面(因为要 pop)
            splited_target_name = target_name.split('_')[::-1]
            # 如果多杯是同一桌, 去掉重复桌号
            self.current_target['target_name'] = sorted(list(set(splited_target_name)), key=splited_target_name.index)
            self.logger.info('目标桌号: %s' % target_name)

            self.center.set_param('/peanut/ready', 'no')    # 告知机械臂花生离开
            self.logger.info('[SEND SIGNAL] /peanut/ready, \'no\' ')
            if use_app:
                self.app.delivery_close_hatch()
                self.out_cabin(stand_by=False)
                self.center.set_param('/peanut/coffee_place', 'no') # 重置机械臂咖啡放置标记
                self.logger.info('[SEND SIGNAL] /peanut/coffee_place, \'no\' ')
            else:
                self.app.close_hatch()
                self.out_cabin(stand_by=False)
                self.center.set_param('/peanut/coffee_place', 'no') # 重置机械臂咖啡放置标记
                self.logger.info('[SEND SIGNAL] /peanut/coffee_place, \'no\' ')
            if timeout > 0:
                return True
            else:
                # 说明是超时离开，实际上取餐失败
                return False
        else:
            self.logger.error("无法取到咖啡.")
            return False
            
    def serve_coffee(self, target_name=None, phone_number=None, use_app=True):
        """执行送餐流程

        Args:
            target_name (string): 送餐位置, 如果是 app 操作则传入 app 中的桌号, 如果是程序控制传入 target key.
            phone_number (string): 手机号, 目前的实现里不使用
        """
        self.stop_charge()
        self.get_coffee(target_name, phone_number, use_app=use_app)

        while len(self.current_target['target_name']) > 0:
            # 先来的订单在列表的末尾
            target_name = "%s号桌" % self.current_target['target_name'].pop()

            if use_app:
                self.app.delivery_input(target_name, phone_number)
            self.logger.info('咖啡送往' + target_name)
        
            weight_before_taken = self.ros.cur_weighing
            self.logger.info('咖啡重量: %.2f' % (weight_before_taken / 10))
            
            self.in_cabin = False
            self.stop_move()    # 重置 ros.goal_reach

            if use_app:
                self.app.delivery_go()
            else:
                self.ros.move_to(self.target_dict[self.current_target['target_name']])
                
            for _ in range(90): # 等待到达目标
                if not self.ros.goal_reached:
                    time.sleep(1)
            
            if self.ros.goal_reached:
                self.logger.info('咖啡送达' + target_name)
                if use_app:
                    self.app.open_hatch_on_delivered(phone_number)      # 打开舱门
                    self.app.device(textContains="关门返回").wait(10)    # 等待 关门返回 按钮被按下
                else:
                    self.app.open_hatch()
                self.logger.info('舱门已打开')
                # 每隔三秒检测重量：
                for _ in range(15):
                    if len(self.current_target['target_name']) > 0:
                        # 如果有多桌, 拿走一杯就离开, 一个空杯子大约是 7 克, 这里设置 6 克的阈值
                        coffee_taken = weight_before_taken - self.ros.cur_weighing > 60
                        weight_before_taken = self.ros.cur_weighing
                    else:
                        # 如果只有一桌, 全部拿走再离开, 误差 2 克
                        coffee_taken = abs(self.ros.cur_weighing - self.init_weight) < 20
                    if coffee_taken:    
                        # 咖啡被取走
                        self.logger.info("咖啡被取走")
                        time.sleep(4)
                        if use_app:
                            self.app.device(textContains="关门返回").click_exists()
                        else:
                            self.close_hatch()
                        self.logger.info('舱门已关闭')
                        break
                    elif use_app and not self.app.device(textContains="关门返回").exists():
                        # 咖啡(可能)没有被取走, 但是舱门已经被关闭
                        self.logger.info("舱门已关闭")
                        break
                    else:
                        time.sleep(3)
                # 送多杯, 退回到送餐界面修改目标点. 如果是用 ROS 走的流程直接重新进入循环就可以
                if len(self.current_target['target_name']) > 0:
                    if use_app:
                        self.app.device.wait_activity(self.app.activities['导航'], timeout=10.0)
                        self.app.device.press('back')
                    continue
            else:
                self.logger.error('无法到达 %s' % target_name)
                return False
        else:
            return True
  
    def run_app(self, times=None, wait_time=5, use_app=True):
        """APP 控制的业务流程

        Args:
            times (int, optional): 测试次数. Defaults to None. 传入 None 则无限循环.
            wait_time (int, optional): 等待订单的时长, 单位为分钟. Defaults to 5.
            use_app (bool, optional): 传入 True 使用 APP 执行业务流程, 否则使用 ROS.
        """
        while times is None or times > 0:
            if self.order_ready or self.wait_for_order(wait_time=wait_time, interval=5):    # 等待订单
                # 等待结束前接到订单，开始送餐
                # 打断所有移动操作（正常流程中只会打断回充）
                self.stop_move()
                self.serve_coffee(use_app=use_app)
                self.order_ready = False
                self.logger.info('送餐结束')
                self.into_cabin()   # 进舱等待
            else:
                # 等待结束前仍未接到订单，返回充电
                self.charge()
                self.order_ready = self.wait_for_order(interval=10)
                self.stop_charge()
            if times is not None:
                times -= 1

device = None
peanut = None
try:
    os.system('adb kill-server')
    os.system('adb start-server')
    os.system('adb connect ' + ANDROID_IP_ADDRESS)
    device = u2.connect_adb_wifi(ANDROID_IP_ADDRESS+':5555')
except Exception:
    print("uiautomator2 无法连接.")
    exit(-1)
if device is not None:
    app = PeanutAPP(device)
    ros = PeanutROS(master_url=ROS_MASTER_IP_ADDRESS)
    peanut = Peanut(app, ros)


# === 注册 atexit 处理器, 请注意两个函数的顺序不能改变 ===
@atexit.register
def disconnect_adb():
    os.system('adb disconnect')
    os.system('adb kill-server')


@atexit.register
def charge_on_exit():
    peanut.charge()


if __name__ == '__main__':
    if peanut is not None:
        peanut.run_app(times=None)
    else:
        print("启动过程中发生错误.")